package Roboter;
import lejos.nxt.Button;
import lejos.nxt.LCD;
import lejos.nxt.Tools;

public class Bewegung {
	private Rotationsachse rotationsachse;
	private Linearachse linearachse;
	private Hubservo hubservo;
	private RotationsPoti rotationsPoti;
	private LinearPoti linearPoti;
	private HubPoti hubPoti;
	private Elektromagnet elektromagnet;
	private boolean weiter;
	
	private static final int anschlagHubOben = 350;
	private static final int anschlagHubUnten = 298;
	private static final int startLinear = 100;
	private static final int startRotation = 10;
	private static final int bremsAbstandLinear = 80;
	private static final int bremsAbstandRotation = 60;
	private static final int zeitVerzoegerung = 500;

	public Bewegung(Linearachse linearachse, LinearPoti linearPoti, Rotationsachse rotationsachse, RotationsPoti rotationsPoti, Hubservo hubservo, HubPoti hubPoti, Elektromagnet elektromagnet) {
		this.linearachse = linearachse;
		this.linearPoti = linearPoti;
		this.rotationsachse = rotationsachse;
		this.rotationsPoti = rotationsPoti;
		this.hubservo = hubservo;
		this.elektromagnet = elektromagnet;
		this.hubPoti = hubPoti;
	}
	
	
	public void startPosition() {
		
		weiter = true;
		while(weiter) {
			if(linearPoti.gibAktuellenWert() < startLinear) {
				this.linearachse.stopp();
				weiter = false;
			}
			else if(linearPoti.gibAktuellenWert() < startLinear + bremsAbstandLinear) {
				this.linearachse.langsamZurueck();
			}
			else if(linearPoti.gibAktuellenWert() > startLinear) {
				this.linearachse.schnellZurueck();
			}
		}
		
		weiter = true;
		while(weiter) {
			if(rotationsPoti.gibAktuellenWert() < startRotation) {
				this.rotationsachse.stopp();
				weiter = false;
			}
			else if(rotationsPoti.gibAktuellenWert() < startRotation + bremsAbstandRotation) {
				this.rotationsachse.langsamZurueck();
			}
			else if(rotationsPoti.gibAktuellenWert() > startRotation) {
				this.rotationsachse.schnellZurueck();
			}
		}
	}


	public boolean wuerfelVerschieben(Position ausgangsPosition, Position zielPosition) {
		if (ausgangsPosition.hatWuerfel()) {
			// Linearachse auf Ausgangsposition ausrichten
			linearachse(ausgangsPosition);

			// Rotationsachse auf Ausgangsposition ausrichten
			rotationsachse(ausgangsPosition);

			// Wuerfel aufheben
			zylinderAktion();
			
			// Linearachse auf Zielposition ausrichten
			linearachse(zielPosition);

			// Rotationsachse auf Zielposition ausrichten
			rotationsachse(zielPosition);
			
			// Wuerfel ablegen
			zylinderAktion();
			
			//Würfel Positionen anpassen
			ausgangsPosition.wechselnWuerfel();
			zielPosition.wechselnWuerfel();
			return true;
		} else {
			System.out.println("Kein Wuerfel auf der Ausgangsposition");
			return false;
		}
	}
	
	public void hubAuf() {
		/*
		this.hubservo.auf();
		try {
			Thread.sleep(200);
		} catch (InterruptedException e) {
			
		}
		this.hubservo.stopp();
		*/
		weiter = true;
		while(weiter) {
			if(hubPoti.gibAktuellenWert() >= anschlagHubOben) {
				this.hubservo.stopp();
				weiter = false;
			}
			
			else if(hubPoti.gibAktuellenWert() < anschlagHubOben) {
				this.hubservo.auf();
			}
		}
		
	}
	
	public void hubAb() {
		/*
		this.hubservo.ab();
		try {
			Thread.sleep(200);
		} catch (InterruptedException e) {
			
		}
		this.hubservo.stopp();
		*/
		weiter = true;
		while(weiter) {
			if(hubPoti.gibAktuellenWert() <= anschlagHubUnten) {
				this.hubservo.stopp();
				weiter = false;
			}
			else if(hubPoti.gibAktuellenWert() > anschlagHubUnten) {
				this.hubservo.ab();
			}
		}
		
	}
	
	private void zylinderAktion() {
		hubAb();
		try {
			Thread.sleep(150);
		} catch (InterruptedException e) {
			
		}
		this.elektromagnet.schalten();
		try {
			Thread.sleep(150);
		} catch (InterruptedException e) {
			
		}
		hubAuf();
	}


	// Linearachse auf Position ausrichten
	private void linearachse(Position ausgangsPosition) {
		weiter = true;

		if (linearPoti.gibAktuellenWert() < ausgangsPosition.gibAchsenKoordinate()) {
			while(weiter) {
				if (linearPoti.gibAktuellenWert() >= ausgangsPosition.gibAchsenKoordinate()) {
					linearachse.stopp();
					weiter = false;
				}
				else if (linearPoti.gibAktuellenWert() > ausgangsPosition.gibAchsenKoordinate() - bremsAbstandLinear) {
					linearachse.langsamVor();
				}
				else if (linearPoti.gibAktuellenWert() < ausgangsPosition.gibAchsenKoordinate()) {
					linearachse.schnellVor();
				}
			}
		} else {
			while(weiter) {
				if (linearPoti.gibAktuellenWert() <= ausgangsPosition.gibAchsenKoordinate()) {
					linearachse.stopp();
					weiter = false;
				}
				else if (linearPoti.gibAktuellenWert() < ausgangsPosition.gibAchsenKoordinate() + bremsAbstandLinear) {
					linearachse.langsamZurueck();
				}
				else if (linearPoti.gibAktuellenWert() > ausgangsPosition.gibAchsenKoordinate()) {
					linearachse.schnellZurueck();
				}
			}
		}
	}
	
	// Rotationsachse auf Position ausrichten
	private void rotationsachse(Position ausgangsPosition) {
		weiter = true;
		
		//if (Math.abs(rotationsPoti.gibAktuellenWert() - ausgangsPosition.gibErsteRotationsKoordinate()) < Math.abs(ausgangsPosition.gibZweiteRotationsKoordinate() - rotationsPoti.gibAktuellenWert())) {
			if(rotationsPoti.gibAktuellenWert() < ausgangsPosition.gibErsteRotationsKoordinate()) {
				while(weiter) {
					if (rotationsPoti.gibAktuellenWert() >= ausgangsPosition.gibErsteRotationsKoordinate()) {
						rotationsachse.stopp();
						weiter = false;
					}
					else if (rotationsPoti.gibAktuellenWert() > ausgangsPosition.gibErsteRotationsKoordinate() - bremsAbstandRotation) {
						rotationsachse.langsamVor();
					}
					else if (rotationsPoti.gibAktuellenWert() < ausgangsPosition.gibErsteRotationsKoordinate()) {
						rotationsachse.schnellVor();
					}
				}
			} else {
				while(weiter) {
					if (rotationsPoti.gibAktuellenWert() <= ausgangsPosition.gibErsteRotationsKoordinate()) {
						rotationsachse.stopp();
						weiter = false;
					}
					else if (rotationsPoti.gibAktuellenWert() < ausgangsPosition.gibErsteRotationsKoordinate() + bremsAbstandRotation) {
						rotationsachse.langsamZurueck();
					}
					else if (rotationsPoti.gibAktuellenWert() > ausgangsPosition.gibErsteRotationsKoordinate()) {
						rotationsachse.schnellZurueck();
					}
				}
			}
		/*} else {
			if(rotationsPoti.gibAktuellenWert() < ausgangsPosition.gibZweiteRotationsKoordinate()) {
				while(weiter) {
					if (rotationsPoti.gibAktuellenWert() == ausgangsPosition.gibZweiteRotationsKoordinate()) {
						rotationsachse.stopp();
						weiter = false;
					}
					else if (rotationsPoti.gibAktuellenWert() > ausgangsPosition.gibZweiteRotationsKoordinate() - bremsAbstandRotation) {
						rotationsachse.langsamVor();
					}
					else if (rotationsPoti.gibAktuellenWert() < ausgangsPosition.gibZweiteRotationsKoordinate()) {
						rotationsachse.schnellVor();
					}
				}
			} else {
				while(weiter) {
					if (rotationsPoti.gibAktuellenWert() == ausgangsPosition.gibZweiteRotationsKoordinate()) {
						rotationsachse.stopp();
						weiter = false;
					}
					else if (rotationsPoti.gibAktuellenWert() < ausgangsPosition.gibZweiteRotationsKoordinate() + bremsAbstandRotation) {
						rotationsachse.langsamZurueck();
					}
					else if (rotationsPoti.gibAktuellenWert() > ausgangsPosition.gibZweiteRotationsKoordinate()) {
						rotationsachse.schnellZurueck();
					}
				}
			}
		}*/
	}
}
